#include "bsp.h"

uint8_t ByteRecv;
uint8_t Serial_RxFlag;

extern uint8_t STATE; //全局变量State机
extern uint8_t DState; //跳出循环控制变量

//串口发送一个字节
void hhSerialSendByte(uint8_t Byte){
	HAL_UART_Transmit(&huart1, &Byte, 1, HAL_MAX_DELAY);
}

//串口发送数组
void hhSerialSendArray(uint8_t* Array,uint16_t Length){
	for(uint16_t i=0;i<Length;i++){
		hhSerialSendByte(Array[i]);
	}
}

//串口发送字符串
void hhSerialSendString(char* mString){
	for(uint16_t i=0;mString[i]!='\0';i++){
		hhSerialSendByte(mString[i]);
	}
}

//没用应该
uint32_t Serial_Pow(uint32_t X,uint32_t Y){
	uint32_t Result=1;
	while(Y--){
		Result*=X;
	}
	return Result;
}

//串口发送数字
void hhSerial_SendNumber(uint32_t Number,uint8_t Length){
	uint8_t i;
	for(i=0;i<Length;i++){
		hhSerialSendByte(Number/Serial_Pow(10, Length-i-1)%10+'0');
	}
}

//串口打印数据
void Serial_Printf(char* format,...){
	char String[100];
	va_list arg;
	va_start(arg,format);
	vsprintf(String,format,arg);
	va_end(arg);
	hhSerialSendString(String);
}

//串口中断标志位处理函数
uint8_t Serial_GetRxFlag(void){
	if(Serial_RxFlag==1){
		Serial_RxFlag=0;
		return 1;
	}
	return 0;
}

//状态切换函数
void LCD_SendByte1(void){
	if(Serial_GetRxFlag()==1){
		DState=1;
	    if(ByteRecv==0x01) STATE=1;
	    if(ByteRecv==0x02) STATE=2;
	    if(ByteRecv==0x03) STATE=3;
	    if(ByteRecv==0x04) STATE=4;
	    if(ByteRecv==0x05) STATE=5;
	    if(ByteRecv==0x06) STATE=0;
	}
}
void LCD_SendByte2(void){
	if(Serial_GetRxFlag()==1){
	    if(ByteRecv==0x06) {
	    	DState=1;
	    	STATE=0;
	    }
	}
}

//发送结束符函数
void SendEnd(void){
	hhSerialSendByte(0xFF);
	hhSerialSendByte(0xFF);
	hhSerialSendByte(0xFF);
}

//串口屏处理函数
void Serial_Send(){
	SendEnd();
	LCD_SendByte2();
}

////激光测距基本操作函数
//uint32_t VL53L0x_CeJu(void){
//	uint32_t distance=0;
//	VL53L0x_StartConversion(&hi2c2); //启动测距转换
//	HAL_Delay(10);
//	VL53L0x_ReadDistance(&hi2c2,&VL53L0x_Bottom);//读取距离数据
//	if(VL53L0x_Bottom.dist - 30>0)
//		distance=VL53L0x_Bottom.dist - 30; //获取数据
//	return distance;
//}

//mode3计算函数

// 余弦定理计算第三边：已知两边a,b及夹角C（默认120度）
float calculate_third_side(uint32_t a, uint32_t b, uint32_t angle) {
    float angle_rad = angle * M_PI / 180.0; // 角度转弧度
    return sqrt(a*a + b*b - 2*a*b*cos(angle_rad));
}

float Calc_mode3_distance(uint32_t distance[], uint32_t angle[]) {
    float min_third_side = FLT_MAX; // 初始化为最大float值

    // 遍历所有距离对
    for (uint32_t i = 0; i < 6; i++) {
        if (distance[i] == 0) continue; // 跳过零值

        for (uint32_t j = i + 1; j < 6; j++) {
            if (distance[j] == 0) continue; // 跳过零值

            // 计算两个角度之间的夹角（取绝对值）
            uint32_t angle_diff = (angle[i] > angle[j]) ?
                                 (angle[i] - angle[j]) :
                                 (angle[j] - angle[i]);

            // 确保夹角在0-180度范围内
            if (angle_diff > 180) {
                angle_diff = 360 - angle_diff;
            }

            // 计算第三边
            float third_side = calculate_third_side(
                distance[i],
                distance[j],
                angle_diff
            );

            // 更新最小值
            if (third_side < min_third_side) {
                min_third_side = third_side;
            }
        }
    }

    // 返回结果（如果没有有效值返回-1）
    return (min_third_side != FLT_MAX) ? min_third_side : -1.0f;
}
